#ifndef     __SIMPLE_STEP_MOTOR_DRV_H
#define     __SIMPLE_STEP_MOTOR_DRV_H

#include "../rtos/event_flags.h"

namespace pub_driver {

enum SimpleDir : uint8_t {

    ENTER = 0x00,
    EXIT = 0x01,
    DIRECT = 0x02,
};

struct SimpleDefaultCfg {
    static void set_dir(SimpleDir dir) {}
    static void set_pwm_freq(uint32_t freq) {}
    static void enable_pwm(uint32_t freq) {}
    static void enable_pwm_not_irq(uint32_t freq) {}
    static void disable_pwm() {}
    static bool is_origin() { return false; }
    static bool is_alm() { return false; }

    static constexpr uint32_t BASE_FREQ = 100;
    static constexpr uint32_t ACC = 2;
    static constexpr uint32_t VAR_RANGE = 200;

    static constexpr uint8_t EC_ALM = 1;
    static constexpr uint8_t EC_TIMEOUT = 2;

    static constexpr uint32_t RESET_PULS_COUNT = 500;
    static constexpr uint32_t RESET_TIMEOUT = 30 * 1000;
};

template <typename CFG>
struct SimpleStepMotorDrv {

public:
    static constexpr uint32_t FLAG_RUN_FINISH = 0x01;

    void init() {
        m_flags.init();
    }

    void on_puls() {
        m_current ++;
        
        if (m_current >= m_total) {
            CFG::disable_pwm();
            m_flags.set_flags(FLAG_RUN_FINISH);
            return;
        }

        if (m_current <= m_s0) {
            CFG::set_pwm_freq(
				CFG::BASE_FREQ + CFG::ACC * m_current
            );
            return;
        }

        if (m_current <= m_s1) {
            return;
        }

        CFG::set_pwm_freq(
			CFG::BASE_FREQ + CFG::ACC * (m_total - m_current)
        );
    }    

    uint8_t run(SimpleDir dir, uint32_t total) {
        if (total == 0) {
            return 0;
        }
        CFG::set_dir(dir);
        run_total(total);
        return wait_for();
    }

    uint8_t wait_for() {
        m_flags.wait_any_flags(FLAG_RUN_FINISH);
        if (CFG::is_alm()) {
            return CFG::EC_ALM;
        }
        return 0;
    }

    void run_total(uint32_t total) {
        if (total < (2 * CFG::VAR_RANGE)) {
            m_s0 = total / 2;
        } else {
            m_s0 = CFG::VAR_RANGE;
        }
        m_s1 = total - m_s0;
        m_total = total;
        m_current = 0;
        CFG::enable_pwm(CFG::BASE_FREQ);
    }

    static uint8_t reset() {
        uint8_t ec = 0;
        if (!CFG::is_origin()) {
            uint32_t time_count = 0;
            CFG::set_dir(SimpleDir::ENTER);
            CFG::enable_pwm_not_irq(CFG::RESET_FREQ);
            for (;;) {
                if (CFG::is_origin()) {
                    break;
                }

                if (CFG::is_alm()) {
                    ec = CFG::EC_ALM;
                    break;
                }

                osDelay(20);
                time_count += 20;

                if (time_count >= CFG::RESET_TIMEOUT) {
                    ec = CFG::EC_TIMEOUT;
                    break;
                }
            }
            CFG::disable_pwm();
            if (ec != 0) {
                return ec;
            }
        }
        return reset_slow();
    }

    static uint8_t reset_slow() {
        uint8_t ec = exit_origin();
        if (ec != 0) {
            return ec;
        }
        osDelay(50);
        return enter_origin();
    }

private:
    static uint8_t exit_origin() {
        uint8_t ec = 0;
        if (!CFG::is_origin()) {
            return ec;
        }
        uint32_t puls_count = 0;
        CFG::set_dir(SimpleDir::EXIT);
        CFG::disable_pwm();
        for (;;) {
            puls_once();
            if (!CFG::is_origin()) {
                break;
            }

            if (CFG::is_alm()) {
                ec = CFG::EC_ALM;
                break;
            }

            puls_count ++;
            if (puls_count >= CFG::RESET_PULS_COUNT) {
                ec = CFG::EC_TIMEOUT;
                break;
            }
        }
        return ec;
    }

    static uint8_t enter_origin() {
        uint8_t ec = 0;
        if (CFG::is_origin()) {
            return ec;
        }
        uint32_t puls_count = 0;
        CFG::set_dir(SimpleDir::ENTER);
        CFG::disable_pwm();
        for (;;) {
            puls_once();
            if (CFG::is_origin()) {
                break;
            }

            if (CFG::is_alm()) {
                ec = CFG::EC_ALM;
                break;
            }

            puls_count ++;
            if (puls_count >= CFG::RESET_PULS_COUNT) {
                ec = CFG::EC_TIMEOUT;
                break;
            }
        }
        return ec;
    }

    static void puls_once() {
        CFG::PULS_PIN::set();
        osDelay(CFG::SLOW_DELAY_MS);
        CFG::PULS_PIN::clr();
        osDelay(CFG::SLOW_DELAY_MS);
    }

private:
    volatile uint32_t m_total;
    volatile uint32_t m_current;
    volatile uint32_t m_s0;
    volatile uint32_t m_s1;
protected:
    rtos::lazy::EventFlags m_flags;
};

}



#endif
